import random
import math
from collections import deque
from pprint import pprint
from math import pi

class AgentBrain():
    def __init__(self):
        """Put any initialization code here."""
        self.nav = Navigate()
    
    def action(self, observation):
        #pprint(observation)
        if observation['respawn']:
            self.nav.setPath(observation['location'][0],observation['location'][1])
        (s,t) = self.nav.getMove(observation['location'][0],observation['location'][1],observation['direction'])
        action = {
            'turn': t,
            'speed': s,
            'shoot': 0
        }
        return action
        
class Navigate:
    def __init__(self):
        #List regions > List points > Tuple x,y,cross
        """self.initList =  [ [ (625,75,1), (625,125,1) ] , \
            [ (625,675,2),(625,725,2) ], \
            [ (275,725,3),(275,775,3),(75,525,4),(25,525,4) ], \
            [ (25,475,4),(75,475,4),(75,325,5),(25,325,5),(275,375,7),(275,425,7) ], \
            [ (25,275,5),(75,275,5),(275,25,6),(275,75,6) ], \
            [ (575,75,1),(575,125,1),(575,675,2),(575,725,2),(325,725,3),
            (325,775,3),(325,375,7),(325,425,7),(325,25,6),(325,75,6) ] ]"""
        self.initList =  [ [ (625,100,1)] , \
            [ (625,700,2) ], \
            [ (275,750,3),(50,525,4) ], \
            [ (50,475,4),(50,325,5),(275,400,7) ], \
            [ (50,275,5),(275,50,6) ], \
            [ (575,100,1),(575,700,2),(325,750,3),(325,400,7),(325,50,6) ] ]
        #Control-points
        self.goalList = [(500,400),(220,220),(220,580)]
        self.constructGraph()
        self.path = deque([])
        self.hist = deque([])

    def constructGraph(self):
        self.regions = []
        for i in self.initList:
            reg = []
            for j in i:
                reg.append(Point(j[0],j[1],j[2],len(self.regions),True))
            for j in reg:
                for k in reg:
                    j.addNeigh(k)
            self.regions.append(reg)
        c = 1
        while True:
            t = []
            for i in self.regions:
                for j in i:
                    if j.cross == c:
                        t.append(j)
            if len(t) == 0:
                break
            for i in t:
                for j in t:
                    i.addNeigh(j)
            c += 1
    
    # gets the region, pretty hard-coded
    def getRegion(self,x,y):
        if x >= 600:
            if y < 400:
                return 0 # region A
            return 1 # region B
        if x >= 300:
            return 5 # region F
        if y >= 500:
            return 2 # region C
        if y >= 300:
            return 3 # region D
        return 4 # region E
    
    def setPath(self,x,y):
        self.hist = deque([])
        g = self.goalList[random.randint(0,2)]
        gr = self.getRegion(g[0],g[1])
        gp = Point(g[0],g[1],0,gr,False)
        cr = self.getRegion(x,y)
        cp = Point(x,y,0,cr,False)
        for i in self.regions[cr]:
            cp.addNeigh(i)
        l = deque([[0,[cp]]])
        minDist = -1
        minPath = []
        while len(l) > 0:
            p = l.popleft()
            if p[1][-1].region == gr:
                d = p[1][-1].getDist(gp)
                if minDist == -1 or p[0] + d < minDist:
                    np = p[1][:]
                    np.append(gp)
                    minDist = p[0] + d
                    minPath = np
            else:
                for i in p[1][-1].neigh:
                    if i[0] not in p:
                        np = p[1][:]
                        np.append(i[0])
                        nd = p[0]+i[1]
                        if minDist == -1 or nd < minDist:
                            l.append([nd,np])
        self.path = deque(minPath)
    
    def getMove(self,x,y,dir):
        cp = Point(x,y,0,self.getRegion(x,y),False)
        dt = 0
        for i in self.hist:
            dt += cp.getDist(i)
        self.hist.append(cp)
        if len(self.hist) > 2:
            self.hist.popleft()
        if dt < 1:
            return (30,random.random()*pi/1.5 - pi/3)
        else:
            d = 0
            a = 0
            while True:
                if len(self.path) == 0:
                    self.setPath(x,y)
                wp = self.path.popleft()
                d = cp.getDist(wp)
                a = cp.getAngle(wp)
                if d < 5:
                    d = 0
                if d > 0:
                    self.path.appendleft(wp)
                    break
            a -= dir
            if abs(a) > math.pi:
                if a >= 0:
                    a -= 2*math.pi
                else:
                    a += 2*math.pi
            if abs(a) > math.pi / 3:
                d = 0
            return (d,a)
    
    def printGraph(self):
        c = 0
        for i in self.regions:
            print c
            for j in i:
                print j.coord
                for k in j.neigh:
                    print k[0].coord,'dist:',k[1]
            c+=1
    
    def printPath(self):
        while len(self.path) > 0:
            n = self.path.popleft()
            print n.coord

class Point:
    def __init__(self,x,y,cross,region,way):
        if way:
            x += random.randint(-20,20)
            y += random.randint(-20,20)
        self.coord = (x,y)
        self.cross = cross
        self.region = region
        self.neigh = []
        self.check = []
    
    def addNeigh(self,n):
        if n != self and n not in self.check:
            d = self.getDist(n)
            self.neigh.append((n,d))
            self.check.append(n)
    
    def getDist(self,n):
        x = n.coord[0]-self.coord[0]
        y = n.coord[1]-self.coord[1]
        d = math.sqrt(x**2+y**2)
        return d
    
    def getAngle(self,n):
        x = n.coord[0]-self.coord[0]
        y = n.coord[1]-self.coord[1]
        a = -math.atan2(-y,x)
        return a